#include "lasercamerainstructionlogic.h"
#include "programengine.h"
#include "messagelog.h"
#define D_DEBUG_FLAG 0
#define D_FETCH_WAIT_COUNT 30
LaserCameraInstructionLogic::LaserCameraInstructionLogic(int robotIdIn, ProgramSet *programSetIn,
        RobotIo *ioControllerIn, ProgramEngine *programEngineIn,
                              McControllerThread *programMcControllerIn, RegData *regDataIn)
{
    programSet=programSetIn;
    robotId=robotIdIn;
    ioController=ioControllerIn;
    programEngine=programEngineIn;
    programMcController=programMcControllerIn;
    regData=regDataIn;
    isInLaserCameraTrack=false;

}

bool LaserCameraInstructionLogic::getLaserCameraTrackStatus()
{
    return isInLaserCameraTrack;
}

int LaserCameraInstructionLogic::reset()
{
    isInLaserCameraTrack=false;
}

E_PROGRAM_LOOP_RESULT LaserCameraInstructionLogic::startFetchWeldPoint(int programNum, int startLine,
                                             const LaserCameraPointFetchInstruction &instruction)
{
    qDebug()<<"LaserCameraInstructionLogic::startFetchWeldPoint";
    PointPro tcpPosition;
    E_PROGRAM_LOOP_RESULT tmpKey=fetchWeldPoint(tcpPosition,D_FETCH_WAIT_COUNT);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpKey)
    {
        return tmpKey;
    }

    regData->setPrRegValue(instruction.prRegisterIndex-1,tcpPosition);
    programEngine->calculateNextLineToRun(programNum);
    return programEngine->isTheLastInstruction(programNum);
}

E_PROGRAM_LOOP_RESULT LaserCameraInstructionLogic::startLaserCameraControl(int programNum, int startLine,
                                                    const LaserCameraCtrlInstruction &ctrlInstruction)
{
    LaserCameraStatusInfo infoOut;
    programEngine->getLaserCameraStatus(infoOut);
    if(1!=infoOut.connectStatus)
    {
        programEngine->reconnectLaserCamera();
        realTimeSleep(1000000);
        programEngine->getLaserCameraStatus(infoOut);
        if(1!=infoOut.connectStatus)
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "LaserCameraInstructionLogic",  12700, robotId );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
    }

    if(ctrlInstruction.isOn)
    {
        programEngine->setLaserCameraShowLaser(true);
        realTimeSleep(50000);
        programEngine->setLaserCameraJobId(ctrlInstruction.jobIndex);
        realTimeSleep(50000);
        programEngine->setLaserCameraWork(true);
        realTimeSleep(50000);
    }
    else
    {
        programEngine->setLaserCameraShowLaser(false);
        realTimeSleep(50000);
        programEngine->setLaserCameraWork(false);
        realTimeSleep(50000);
    }


    int maxWait=80;
    do
    {
        realTimeSleep(50000);
        programEngine->getLaserCameraStatus(infoOut);
        if(1!=infoOut.connectStatus)
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "LaserCameraInstructionLogic",  12700, robotId );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        maxWait-=1;
        if(0>maxWait)
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "LaserCameraInstructionLogic",  12701, robotId );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        if(D_DEBUG_FLAG)
        {
            qDebug()<<"ctrlInstruction.jobIndex!=infoOut.jobId LaserCameraInstructionLogic wait for "<<infoOut.regReadSucessCount;
        }


    }
    while(ctrlInstruction.jobIndex!=infoOut.jobId);

    programEngine->calculateNextLineToRun(programNum);
    return programEngine->isTheLastInstruction(programNum);

}

E_PROGRAM_LOOP_RESULT LaserCameraInstructionLogic::startLaserCameraTrack(int programNum, int startLine,
                                                    const LaserCameraTrackInstruction &trackInstruction)
{
    qDebug()<<"LaserCameraInstructionLogic::startLaserCameraTrack";
    isInLaserCameraTrack=trackInstruction.isStart;
    programEngine->calculateNextLineToRun(programNum);
    return programEngine->isTheLastInstruction(programNum);

}

E_PROGRAM_LOOP_RESULT LaserCameraInstructionLogic::fetchWeldPoint(PointPro &tcpPositionOut, int waitCountIn)
{
    LaserCameraStatusInfo infoOut;
    int maxWait=waitCountIn;
    do
    {
        realTimeSleep(50000);
        programEngine->getLaserCameraStatus(infoOut);
        if(1!=infoOut.connectStatus)
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "LaserCameraInstructionLogic",  12700, robotId );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        maxWait-=1;
        if(0>maxWait)
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "LaserCameraInstructionLogic",  12702, robotId );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        if(D_DEBUG_FLAG)
        {
            qDebug()<<"bbb LaserCameraInstructionLogic  regReadSucessCount"<<infoOut.regReadSucessCount;
        }


    }
    while(255!=infoOut.isFind);

    if(255!=infoOut.isFind)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "LaserCameraInstructionLogic",  12702, robotId );
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }


    programMcController->mcControllerInterpreter->getCurrentCoordinatePoint_Feedback(tcpPositionOut);

    tcpPositionOut.positionValue[0]=infoOut.world_x;
    tcpPositionOut.positionValue[1]=infoOut.world_y;
    tcpPositionOut.positionValue[2]=infoOut.world_z;
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT LaserCameraInstructionLogic::laserScanStart(int programNum, bool isStartIn)
{
    if(isStartIn)
    {
        programEngine->startLaserScanBuffer();
    }
    else
    {
        programEngine->stopLaserScanBuffer();
    }
    programEngine->calculateNextLineToRun(programNum);
    return programEngine->isTheLastInstruction(programNum);
}

void LaserCameraInstructionLogic::realTimeSleep(long long umicoroSecond)
{
    // 处理qt事件
    QCoreApplication::processEvents();

    struct timespec time;
    clock_gettime(CLOCK_REALTIME,&time);
    time.tv_nsec += umicoroSecond * 1000;
    while(time.tv_nsec >= NSEC_PER_SEC)
    {
        time.tv_nsec -= NSEC_PER_SEC;
        ++time.tv_sec;
    }
    clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&time,NULL);
}

void LaserCameraInstructionLogic::addMsg(int messageLevel, string componentName,
                                       string messageType, int messageCode, int robotId, int parameter1,
                                       int parameter2, int parameter3, int parameter4, QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {
    case 12702:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",激光相机未识别出焊缝！")+message;
    }
        break;
    case 12701:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",激光相机超时未应答！")+message;
    }
        break;
    case 12700:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",激光相机处于离线状态！")+message;
    }
        break;


    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}

